#include "yolov5n.h"

int input_size = 1 * 3 * 384 * 640;
const char model_name[] = "network";
int outputWidth = 640;
int outputHeight = 384;
int bit_depth = 8;
int color_type = PNG_COLOR_TYPE_RGB;
// PngData png_data;
int fd;
struct v4l2_fmtdesc fmtdesc;
struct v4l2_frmsizeenum fsenum;
int fmt_index = 0;
int frame_index = 0;
int i;
void *bufs[32];
int buf_cnt;
int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
struct pollfd fds[1];
char filename[] = "result.bmp";
// int file_cnt = 0;

int result_x[RESULT_NUM];
int result_y[RESULT_NUM];
int result_w[RESULT_NUM];
int result_h[RESULT_NUM];
int result_label[RESULT_NUM];
int result_score[RESULT_NUM];
int result_num = 0;

int sockfd;
struct sockaddr_in serveraddr;
socklen_t serveraddr_len;
const float conf_thres = 0.25f;
const float iou_thres = 0.45f;
struct shl_yolov5_params *params1;
int start_flag = 1;
struct csinn_tensor *output_tensors[3];
struct shl_yolov5_box out[32];

int fd_hzk16;
struct stat hzk_stat;
unsigned char *hzkmem;
uint8_t *packet;
// 发送线程的属性
pthread_attr_t attr;
struct sched_param param;
// 初始化变量
void *sess;
struct csinn_tensor *input_tensors[1];
uint8_t* rgbData = NULL;
void *input_aligned =NULL;
uint8_t* scaledRGBData = NULL;
uint64_t start_time, end_time;
int file_cnt = 0;
int counter = 0;
int start_flag1 = 1;

static void postprocess_opt(void *sess)
{
    int output_num;
    output_num = csinn_get_output_number(sess);
    if (start_flag == 1)
    {
        params1 = (struct shl_yolov5_params *)shl_mem_alloc(sizeof(struct shl_yolov5_params));
        params1->conf_thres = conf_thres;
        params1->iou_thres = iou_thres;
        params1->strides[0] = 8;
        params1->strides[1] = 16;
        params1->strides[2] = 32;
        float anchors[18] = {10.f, 13.f, 16.f, 30.f, 33.f, 23.f,
                            30.f, 61.f, 62.f, 45.f, 59.f, 119.f,
                            116.f, 90.f, 156.f, 198.f, 373.f, 326.f};
        memcpy(params1->anchors, anchors, sizeof(anchors));
        start_flag = 0;
        output_tensors[0] = csinn_alloc_tensor(NULL);
        output_tensors[1] = csinn_alloc_tensor(NULL);
        output_tensors[2] = csinn_alloc_tensor(NULL);
    }

    for (int i = 0; i < output_num; i++)
    {
        output_tensors[i]->data = NULL;
        csinn_get_output(i, output_tensors[i], sess);

        if (output_tensors[i]->qinfo != NULL)
        {
            shl_mem_free(output_tensors[i]->qinfo);
            output_tensors[i]->qinfo = NULL;
        }
        output_tensors[i]->quant_channel = 0;
        output_tensors[i]->dtype = CSINN_DTYPE_FLOAT32;
        output_tensors[i]->data = shl_c920_output_to_f32_dtype(i, output_tensors[i]->data, sess);
    }

    int num;
    num = shl_c920_detect_yolov5_postprocess(output_tensors, out, params1);
    int i = 0;

    if(num != 0)
    {
        printf("detect num: %d\n", num);
        result_num = num;
        printf("id:\tlabel\tscore\t\tx1\t\ty1\t\tx2\t\ty2\n");
        for (int k = 0; k < num; k++)
        {
            printf("[%d]:\t%d\t%f\t%f\t%f\t%f\t%f\n", k, out[k].label,
                out[k].score, out[k].x1, out[k].y1, out[k].x2, out[k].y2);
        
            result_x[k] = (int)out[k].x1;
            result_y[k] = (int)out[k].y1;
            result_w[k] = (int)(out[k].x2 - out[k].x1);
            result_h[k] = (int)(out[k].y2 - out[k].y1);
            result_label[k] = (int)out[k].label;
            result_score[k] = (int)(out[k].score * 100);
        }
    }

    // shl_mem_free(params1);

    // for (int i = 0; i < 3; i++)
    // {
    //     csinn_free_tensor(output_tensors[i]);
    // }
}

void *create_graph(char *params_path)
{
    int binary_size;
    char *params = get_binary_from_file(params_path, &binary_size);
    if (params == NULL)
    {
        return NULL;
    }

    char *suffix = params_path + (strlen(params_path) - 7);
    if (strcmp(suffix, ".params") == 0)
    {
        // create general graph
        return csinn_(params);
    }

    suffix = params_path + (strlen(params_path) - 3);
    if (strcmp(suffix, ".bm") == 0)
    {
        struct shl_bm_sections *section = (struct shl_bm_sections *)(params + 4128);
        if (section->graph_offset)
        {
            return csinn_import_binary_model(params);
        }
        else
        {
            return csinn_(params + section->params_offset * 4096);
        }
    }
    else
    {
        return NULL;
    }
}

int init_camera(char * cam_name)
{
     /* open */
     printf("cam_name=%s\n",cam_name);
    fd = open(cam_name, O_RDWR);
    if (fd < 0)
    {
        printf("can not open %s\n", FILE_VIDEO);
        return -1;
    }

    /* 查询能力 */
    struct v4l2_capability cap;
    memset(&cap, 0, sizeof(struct v4l2_capability));
    
    if (0 == ioctl(fd, VIDIOC_QUERYCAP, &cap))
    {
        printf("driver:\t\t%s\n", cap.driver);
        printf("card:\t\t%s\n", cap.card);
        printf("bus_info:\t%s\n", cap.bus_info);
        printf("version:\t%d\n", cap.version);
        printf("capabilities:\t%x\n", cap.capabilities); 
        if((cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) == 0) {
            fprintf(stderr, "Error opening device %s: video capture not supported.\n",
                    FILE_VIDEO);
            return -1;
        }
        
        if(!(cap.capabilities & V4L2_CAP_STREAMING)) {
            fprintf(stderr, "%s does not support streaming i/o\n", FILE_VIDEO);
            return -1;
        }
    }
    else
    {
        printf("can not get capability\n");
        return -1;
    }

    printf("Support format:\n");
    while (1)
    {
        /* 枚举格式 */
        fmtdesc.index = fmt_index;  // 比如从0开始
        fmtdesc.type  = V4L2_BUF_TYPE_VIDEO_CAPTURE;  // 指定type为"捕获"
        if (0 != ioctl(fd, VIDIOC_ENUM_FMT, &fmtdesc))
            break;
        // printf("\t%d.%s\n", fmtdesc.index + 1, fmtdesc.description);
        frame_index = 0;
        while (1)
        {
            /* 枚举这种格式所支持的帧大小 */
            memset(&fsenum, 0, sizeof(struct v4l2_frmsizeenum));
            fsenum.pixel_format = fmtdesc.pixelformat;
            fsenum.index = frame_index;
            
            if (ioctl(fd, VIDIOC_ENUM_FRAMESIZES, &fsenum) == 0)
            {
                printf("format %s,%d, framesize %d: %d x %d\n", fmtdesc.description, fmtdesc.pixelformat, frame_index, fsenum.discrete.width, fsenum.discrete.height);
            }
            else
            {
                break;
            }

            frame_index++;
        }

        fmt_index++;
    }
    /* 设置格式 */
    struct v4l2_format fmt;
    memset(&fmt, 0, sizeof(struct v4l2_format));
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    fmt.fmt.pix.width = IMAGEWIDTH;
    fmt.fmt.pix.height = IMAGEHEIGHT;
    // fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
    // fmt.fmt.pix.field = V4L2_FIELD_ANY;
    fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
    if (0 == ioctl(fd, VIDIOC_S_FMT, &fmt))
    {
        printf("set format ok: %d x %d\n", fmt.fmt.pix.width, fmt.fmt.pix.height);
    }
    else
    {
        printf("can not set format\n");
        return -1;
    }

    /*
     * 申请buffer
     */
    struct v4l2_requestbuffers rb;
    memset(&rb, 0, sizeof(struct v4l2_requestbuffers));
    rb.count = 4;
    rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    rb.memory = V4L2_MEMORY_MMAP;

    if (0 == ioctl(fd, VIDIOC_REQBUFS, &rb))
    {
        /* 申请成功后, mmap这些buffer */
        buf_cnt = rb.count;
        for(i = 0; i < rb.count; i++) {
            struct v4l2_buffer buf;
            memset(&buf, 0, sizeof(struct v4l2_buffer));
            buf.index = i;
            buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
            buf.memory = V4L2_MEMORY_MMAP;
            if (0 == ioctl(fd, VIDIOC_QUERYBUF, &buf))
            {
                /* mmap */
                bufs[i] = mmap(0 /* start anywhere */ ,
                                  buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, fd,
                                  buf.m.offset);
                if(bufs[i] == MAP_FAILED) {
                    perror("Unable to map buffer");
                    return -1;
                }
            }
            else
            {
                printf("can not query buffer\n");
                return -1;
            }            
        }

        printf("map %d buffers ok\n", buf_cnt);
        
    }
    else
    {
        printf("can not request buffers\n");
        return -1;
    }

    /* 把所有buffer放入"空闲链表" */
    for(i = 0; i < buf_cnt; ++i) {
        struct v4l2_buffer buf;
        memset(&buf, 0, sizeof(struct v4l2_buffer));
        buf.index = i;
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        if (0 != ioctl(fd, VIDIOC_QBUF, &buf))
        {
            perror("Unable to queue buffer");
            return -1;
        }
    }
    printf("queue buffers ok\n");
    /* 启动摄像头 */
    if (0 != ioctl(fd, VIDIOC_STREAMON, &type))
    {
        perror("Unable to start capture");
        return -1;
    }
    printf("start capture ok\n");
}

void png_write_memory_callback(png_structp png_ptr, png_bytep data, png_size_t length) {
    PngData *png_data = (PngData*)png_get_io_ptr(png_ptr);
    png_data->data = (png_bytep)realloc(png_data->data, png_data->size + length);
    if (!png_data->data) {
        fprintf(stderr, "Memory allocation failed\n");
        png_destroy_write_struct(&png_ptr, NULL);
        return;
    }
    memcpy(png_data->data + png_data->size, data, length);
    png_data->size += length;
}

PngData convert_rgb_to_png(int width, int height, int bit_depth, int color_type, uint8_t *rgb_data) {
    PngData png_data = {NULL, 0};

    png_structp png_ptr = png_create_write_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);
    if (!png_ptr) {
        fprintf(stderr, "Could not create PNG write structure\n");
        return png_data;
    }

    png_infop info_ptr = png_create_info_struct(png_ptr);
    if (!info_ptr) {
        fprintf(stderr, "Could not create PNG info structure\n");
        png_destroy_write_struct(&png_ptr, NULL);
        return png_data;
    }

    if (setjmp(png_jmpbuf(png_ptr))) {
        fprintf(stderr, "Error during PNG creation\n");
        png_destroy_write_struct(&png_ptr, &info_ptr);
        return png_data;
    }

    // 设置写入内存的回调函数
    png_set_write_fn(png_ptr, &png_data, png_write_memory_callback, NULL);

    // 分配内存用于存储行指针数组
    png_bytep *row_pointers = (png_bytep*)malloc(height * sizeof(png_bytep));
    for (int y = 0; y < height; y++) {
        row_pointers[y] = (png_bytep)malloc(3 * width * sizeof(png_byte));
        // 填充行指针数组
        for (int x = 0; x < width; x++) {
            row_pointers[y][3 * x] = rgb_data[(y * width + x) * 3];         // 红色通道
            row_pointers[y][3 * x + 1] = rgb_data[(y * width + x) * 3 + 1]; // 绿色通道
            row_pointers[y][3 * x + 2] = rgb_data[(y * width + x) * 3 + 2]; // 蓝色通道
        }
    }

    png_set_IHDR(png_ptr, info_ptr, width, height, bit_depth, color_type, PNG_INTERLACE_NONE, PNG_COMPRESSION_TYPE_DEFAULT, PNG_FILTER_TYPE_DEFAULT);

    png_set_rows(png_ptr, info_ptr, row_pointers);

    png_write_png(png_ptr, info_ptr, PNG_TRANSFORM_IDENTITY, NULL);

    png_destroy_write_struct(&png_ptr, &info_ptr);

    // 释放内存
    for (int y = 0; y < height; y++) {
        free(row_pointers[y]);
    }
    free(row_pointers);

    return png_data;
}

png_bytep* convert_to_png_bytep(uint8_t* scaledRGBData, int width, int height) {
    // 创建一个指向指针数组的指针
    png_bytep *row_pointers = (png_bytep*)malloc(height * sizeof(png_bytep));

    if (!row_pointers) {
        fprintf(stderr, "Memory allocation failed\n");
        return NULL;
    }

    // 将每一行的指针存储到指针数组中
    for (int y = 0; y < height; y++) {
        row_pointers[y] = (png_bytep)(scaledRGBData + y * width * 3); // 假设每个像素包含三个字节（RGB）
    }

    return row_pointers;
}

void write_png_file(const char *filename, int width, int height, int bit_depth, int color_type, png_bytep *row_pointers) {
    FILE *fp = fopen(filename, "wb");
    if (!fp) {
        fprintf(stderr, "Could not open file %s for writing\n", filename);
        return;
    }

    png_structp png_ptr = png_create_write_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);
    if (!png_ptr) {
        fprintf(stderr, "Could not create PNG write structure\n");
        fclose(fp);
        return;
    }

    png_infop info_ptr = png_create_info_struct(png_ptr);
    if (!info_ptr) {
        fprintf(stderr, "Could not create PNG info structure\n");
        png_destroy_write_struct(&png_ptr, (png_infopp)NULL);
        fclose(fp);
        return;
    }

    if (setjmp(png_jmpbuf(png_ptr))) {
        fprintf(stderr, "Error during PNG creation\n");
        png_destroy_write_struct(&png_ptr, &info_ptr);
        fclose(fp);
        return;
    }

    png_init_io(png_ptr, fp);

    png_set_IHDR(png_ptr, info_ptr, width, height, bit_depth, color_type, PNG_INTERLACE_NONE, PNG_COMPRESSION_TYPE_DEFAULT, PNG_FILTER_TYPE_DEFAULT);

    png_set_rows(png_ptr, info_ptr, row_pointers);

    png_write_png(png_ptr, info_ptr, PNG_TRANSFORM_IDENTITY, NULL);

    png_destroy_write_struct(&png_ptr, &info_ptr);
    fclose(fp);
}

// 保存 RGB 数据为 BMP 文件
void SaveRGBToBMP(const char* filename, const uint8_t* data, int width, int height) {
    FILE* file = fopen(filename, "wb");

    uint32_t headers[13] = {
        0,
        width * height * 3 + 54,
        54,
        40,
        width,
        height,
        (24 << 16),
        0,
        width * height * 3,
        0,
        0,
        0,
        0
    };

    fwrite("BM", 1, 2, file);
    fwrite(headers, sizeof(uint32_t), 13, file);

    for (int i = 0; i < width * height; i++) {
        fwrite(&data[i * 3 + 2], 1, 1, file); // Blue
        fwrite(&data[i * 3 + 1], 1, 1, file); // Green
        fwrite(&data[i * 3], 1, 1, file);     // Red
    }

    printf("Done\n");

    fclose(file);
}

int close_v4l2(void)
{                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       
    ioctl(fd, VIDIOC_STREAMOFF, &type); /* 关闭视频流 */
    if (fd != -1)
    {
        close(fd);
        return 1;
    }
    return -1;
}

// 定义将 YUYV 格式数据转换为 RGB 格式的函数
void YUYVToRGB(const uint8_t* yuyv, int width, int height, uint8_t* rgb) {
    int size = width * height;
    for (int i = 0, j = 0; i < size * 2; i += 4, j += 6) {
        int Y1 = yuyv[i];
        int U = yuyv[i + 1];
        int Y2 = yuyv[i + 2];
        int V = yuyv[i + 3];

        // 转换第一个像素
        int R1 = Y1 + 1.402 * (V - 128);
        int G1 = Y1 - 0.344136 * (U - 128) - 0.714136 * (V - 128);
        int B1 = Y1 + 1.772 * (U - 128);

        // 转换第二个像素
        int R2 = Y2 + 1.402 * (V - 128);
        int G2 = Y2 - 0.344136 * (U - 128) - 0.714136 * (V - 128);
        int B2 = Y2 + 1.772 * (U - 128);

        // 将 RGB 值限定在 0 到 255 之间
        R1 = limitToRange(R1);
        G1 = limitToRange(G1);
        B1 = limitToRange(B1);
        R2 = limitToRange(R2);
        G2 = limitToRange(G2);
        B2 = limitToRange(B2);

        // 将 RGB 值存储到输出数组中
        rgb[j] = R1;
        rgb[j + 1] = G1;
        rgb[j + 2] = B1;
        rgb[j + 3] = R2;
        rgb[j + 4] = G2;
        rgb[j + 5] = B2;
    }
}

uint8_t limitToRange(uint8_t value) {
    if (value < 0)
        return 0;
    else if (value > 255)
        return 255;
    else
        return value;
}

void image_preprocess(const uint8_t* image, int height, int width, int target_height, int target_width, uint8_t* image_padded) {
    int ih = target_height;
    int iw = target_width;

    int h = height;
    int w = width;

    float scale = (iw / (float)w) < (ih / (float)h) ? (iw / (float)w) : (ih / (float)h);
    int nw = scale * w;
    int nh = scale * h;

    uint8_t* image_resized = (uint8_t*)malloc(nw * nh * 3 * sizeof(uint8_t));

    // Resize the image
    for (int c = 0; c < 3; ++c) {
        for (int dst_y = 0; dst_y < nh; ++dst_y) {
            for (int dst_x = 0; dst_x < nw; ++dst_x) {
                int src_x = (dst_x + 0.5) / scale - 0.5;
                int src_y = (dst_y + 0.5) / scale - 0.5;
                if (src_x >= 0 && src_x < w && src_y >= 0 && src_y < h) {
                    int dst_idx = (dst_y * nw + dst_x) * 3 + c;
                    int src_idx = (src_y * w + src_x) * 3 + c;
                    image_resized[dst_idx] = image[src_idx];
                }
            }
        }
    }

    int dw = (iw - nw) / 2;
    int dh = (ih - nh) / 2;

    // Pad the image
    for (int c = 0; c < 3; ++c) {
        for (int y = 0; y < ih; ++y) {
            for (int x = 0; x < iw; ++x) {
                if (x >= dw && x < dw + nw && y >= dh && y < dh + nh) {
                    int padded_idx = (y * iw + x) * 3 + c;
                    int resized_idx = ((y - dh) * nw + (x - dw)) * 3 + c;
                    image_padded[padded_idx] = image_resized[resized_idx];
                } else {
                    int padded_idx = (y * iw + x) * 3 + c;
                    image_padded[padded_idx] = 128;
                }
            }
        }
    }

    free(image_resized);
}

// 等比缩放 RGB 数据并补零
void ScaleAndPadRGB(const uint8_t* input, int inputWidth, int inputHeight, uint8_t* output, int outputWidth, int outputHeight) {
    float scaleX = (float)(inputWidth) / outputWidth;
    float scaleY = (float)(inputHeight) / outputHeight;

    for (int y = 0; y < outputHeight; ++y) {
        for (int x = 0; x < outputWidth; ++x) {
            int inputX = (int)(x * scaleX);
            int inputY = (int)(y * scaleY);

            if (inputX >= inputWidth || inputY >= inputHeight) {
                output[(y * outputWidth + x) * 3] = 128;
                output[(y * outputWidth + x) * 3 + 1] = 128;
                output[(y * outputWidth + x) * 3 + 2] = 128;
            } else {
                output[(y * outputWidth + x) * 3] = input[(inputY * inputWidth + inputX) * 3];
                output[(y * outputWidth + x) * 3 + 1] = input[(inputY * inputWidth + inputX) * 3 + 1];
                output[(y * outputWidth + x) * 3 + 2] = input[(inputY * inputWidth + inputX) * 3 + 2];
            }
        }
    }
}

// 在 RGB 数据上绘制矩形框
void DrawRectangle(uint8_t* data, int width, int height, int x, int y, int rectWidth, int rectHeight) {
    for (int i = 0; i < rectHeight; ++i) {
        if (y + i < 0 || y + i >= height) {
            continue;
        }

        for (int j = 0; j < rectWidth; ++j) {
            if (x + j < 0 || x + j >= width) {
                continue;
            }

            if (i == 0 || i == rectHeight - 1 || j == 0 || j == rectWidth - 1) {
                // 绘制矩形框边界
                data[((y + i) * width + x + j) * 3] = 255;
                data[((y + i) * width + x + j) * 3 + 1] = 0;
                data[((y + i) * width + x + j) * 3 + 2] = 0;
            } else {
                // 填充矩形框内部
                data[((y + i) * width + x + j) * 3] = 255;
                data[((y + i) * width + x + j) * 3 + 1] = 255;
                data[((y + i) * width + x + j) * 3 + 2] = 255;
            }
        }
    }
}

// 在 RGB 数据上绘制矩形框边缘
void DrawRectangleOutline(uint8_t* data, int width, int height, int x, int y, int rectWidth, int rectHeight) {
    // 绘制上边框
    for (int i = 0; i < rectWidth; ++i) {
        int posX = x + i;
        int posY = y;
        if (posX >= 0 && posX < width && posY >= 0 && posY < height) {
            data[(posY * width + posX) * 3] = 0;  // R
            data[(posY * width + posX) * 3 + 1] = 0;  // G
            data[(posY * width + posX) * 3 + 2] = 255;  // B
        }
    }

    // 绘制下边框
    for (int i = 0; i < rectWidth; ++i) {
        int posX = x + i;
        int posY = y + rectHeight - 1;
        if (posX >= 0 && posX < width && posY >= 0 && posY < height) {
            data[(posY * width + posX) * 3] = 0;  // R
            data[(posY * width + posX) * 3 + 1] = 0;  // G
            data[(posY * width + posX) * 3 + 2] = 255;  // B
        }
    }

    // 绘制左边框
    for (int i = 0; i < rectHeight; ++i) {
        int posX = x;
        int posY = y + i;
        if (posX >= 0 && posX < width && posY >= 0 && posY < height) {
            data[(posY * width + posX) * 3] = 0;  // R
            data[(posY * width + posX) * 3 + 1] = 0;  // G
            data[(posY * width + posX) * 3 + 2] = 255;  // B
        }
    }

    // 绘制右边框
    for (int i = 0; i < rectHeight; ++i) {
        int posX = x + rectWidth - 1;
        int posY = y + i;
        if (posX >= 0 && posX < width && posY >= 0 && posY < height) {
            data[(posY * width + posX) * 3] = 0;  // R
            data[(posY * width + posX) * 3 + 1] = 0;  // G
            data[(posY * width + posX) * 3 + 2] = 255;  // B
        }
    }
}

void FlipImageHorizontal( uint8_t* data, int width, int height) {
        int channels = 3; // RGB channels
    uint8_t* tempPixel = (uint8_t*)malloc(channels * sizeof(uint8_t));

    for (int row = 0; row < height; ++row) {
        for (int col = 0; col < width / 2; ++col) {
            int oppositeCol = width - 1 - col;

            // 交换当前像素和相对应的对称像素
            for (int c = 0; c < channels; ++c) {
                tempPixel[c] = data[row * width * channels + col * channels + c];
                data[row * width * channels + col * channels + c] = data[row * width * channels + oppositeCol * channels + c];
                data[row * width * channels + oppositeCol * channels + c] = tempPixel[c];
            }
        }
    }

    free(tempPixel);
}

// 转换成模型要求存储格式
void transpose_image_int(const uint8_t* input, uint8_t* output, int height, int width, int channels) {
    for (int c = 0; c < channels; c++) {
        for (int h = 0; h < height; h++) {
            for (int w = 0; w < width; w++) {
                output[c * height * width + h * width + w] = input[h * width * channels + w * channels + c]-128;
            }
        }
    }
}

char* Int2String(int num,char *str)//10进制 
{
	int i = 0;//指示填充str 
	if(num<0)//如果num为负数，将num变正 
	{
		num = -num;
		str[i++] = '-';
	} 
	//转换 
	do
	{
		str[i++] = num%10+48;//取num最低位 字符0~9的ASCII码是48~57；简单来说数字0+48=48，ASCII码对应字符'0' 
		num /= 10;//去掉最低位	
	}while(num);//num不为0继续循环
	
	str[i] = '\0';
	
	//确定开始调整的位置 
	int j = 0;
	if(str[0]=='-')//如果有负号，负号不用调整 
	{
		j = 1;//从第二位开始调整 
		++i;//由于有负号，所以交换的对称轴也要后移1位 
	}
	//对称交换 
	for(;j<i/2;j++)
	{
		//对称交换两端的值 其实就是省下中间变量交换a+b的值：a=a+b;b=a-b;a=a-b; 
		str[j] = str[j] + str[i-1-j];
		str[i-1-j] = str[j] - str[i-1-j];
		str[j] = str[j] - str[i-1-j];
	} 
	return str;//返回转换后的值 
}

// 线程函数，参数为线程的标识符
void* sendYolov5ImageToTargetHostThread(void* image_data) {
    int data_length = 737280;
    int current_packet_index = 0;
    int packet_count = (data_length + PACKET_UNIT - 1) / PACKET_UNIT;
    char packet_index[4];
    for (int i = 0; i < packet_count; i++) {
        // 判断是否为最后一个数据包
        int is_last_packet = (current_packet_index == packet_count - 1);
        // 构建数据包
        int packet_size = (data_length - current_packet_index * PACKET_UNIT > PACKET_UNIT) ? PACKET_UNIT : (data_length - current_packet_index * PACKET_UNIT);
        
         // 清空内存块中的内容
        memset(packet, 0, packet_size + 4);
        Int2String(current_packet_index,packet_index);
        memcpy(packet, packet_index, 4);
        memcpy(packet+4, (uint8_t *)image_data + current_packet_index * PACKET_UNIT, packet_size);

        if(-1 == sendto(sockfd, packet, packet_size+4, 0, (struct sockaddr*)&serveraddr, serveraddr_len))
        {
            printf("send error\n");
        }
        current_packet_index++;
    }
    pthread_exit(NULL); // 线程结束
}

          
void* inferenceThread() {
    csinn_update_input_and_run(input_tensors, sess);
    pthread_exit(NULL); // 线程结束
}

void* postProcessThread() {
    postprocess_opt(sess);
    pthread_exit(NULL); // 线程结束
}

void sendYolov5ImageToTargetHost(const uint8_t *image_data, int data_length) {

    int current_packet_index = 0;
    int packet_count = (data_length + PACKET_UNIT - 1) / PACKET_UNIT;
    char packet_index[4];
    // // if (start_flag1)
    // // {
    //     // 获取红色、绿色和蓝色通道的值
    //     uint8_t red = image_data[0];
    //     uint8_t green = image_data[0 + 1];
    //     uint8_t blue = image_data[0 + 2];

    //     // 打印RGB值
    //     printf("Pixel at position (%d, %d): Red = %d, Green = %d, Blue = %d\n",
    //         0, 0, red, green, blue);
    // //         start_flag1 = 0;
    // // }
    for (int i = 0; i < packet_count; i++) {
        // 判断是否为最后一个数据包
        int is_last_packet = (current_packet_index == packet_count - 1);
        // 构建数据包
        int packet_size = (data_length - current_packet_index * PACKET_UNIT > PACKET_UNIT) ? PACKET_UNIT : (data_length - current_packet_index * PACKET_UNIT);
        // uint8_t *packet = (uint8_t *)malloc(packet_size+4);
        memset(packet, 0, packet_size + 4);
        Int2String(current_packet_index,packet_index);
        memcpy(packet, packet_index, 4);
        memcpy(packet+4, image_data + current_packet_index * PACKET_UNIT, packet_size);

        if(-1 == sendto(sockfd, packet, packet_size+4, 0, (struct sockaddr*)&serveraddr, serveraddr_len))
        {
            printf("send error\n");
        }
        current_packet_index++;
        usleep(100);
    }
}

void init_socket()
{
    sockfd = socket(AF_INET, SOCK_DGRAM, 0);
    if (-1 == sockfd)
    {
        printf("create socket error\n");
    }
    // 填充网络信息结构体

    memset(&serveraddr, 0, sizeof(serveraddr));
    serveraddr.sin_family = AF_INET;

    //端口号需要转换成网络字节序 
    serveraddr.sin_port = htons(PORT);  
    //本地测试时也可以使用127.0.0.1本地环回地址
    //并且是网络字节序的 
    serveraddr.sin_addr.s_addr = inet_addr("127.0.0.1"); 

    serveraddr_len = sizeof(serveraddr);
}

void RGBToBGR(const uint8_t* rgb, int width, int height, uint8_t* bgr) {
    int size = width * height;
    for (int i = 0; i < size * 3; i += 3) {
        // 依次将 RGB 转换为 BGR
        bgr[i] = rgb[i + 2];  // 存储蓝色分量
        bgr[i + 1] = rgb[i + 1];  // 存储绿色分量
        bgr[i + 2] = rgb[i];  // 存储红色分量
    }
}

void YUYVToBGR(const uint8_t* yuyvData, uint8_t* bgrData, int width, int height) {
    int i, yIndex, uIndex, vIndex;
    int pixelCount = width * height;
    int bgrIndex = 0;

    for (i = 0; i < pixelCount; i++) {
        yIndex = i * 2;
        uIndex = yIndex + 1;
        vIndex = uIndex + 1;

        // 获取 Y、U、V 分量的值
        uint8_t y = yuyvData[yIndex];
        uint8_t u = yuyvData[uIndex];
        uint8_t v = yuyvData[vIndex];

        // 转换 YUV 到 RGB
        int c = (int)y - 16;
        int d = (int)u - 128;
        int e = (int)v - 128;
        int r = (298 * c + 409 * e + 128) >> 8;
        int g = (298 * c - 100 * d - 208 * e + 128) >> 8;
        int b = (298 * c + 516 * d + 128) >> 8;

        // 确保 R、G、B 的值在合法范围内（0-255）
        r = r < 0 ? 0 : (r > 255 ? 255 : r);
        g = g < 0 ? 0 : (g > 255 ? 255 : g);
        b = b < 0 ? 0 : (b > 255 ? 255 : b);

        // 将 RGB 数据存储到 BGR 数组中
        bgrData[bgrIndex + 0] = b;
        bgrData[bgrIndex + 1] = g;
        bgrData[bgrIndex + 2] = r;

        bgrIndex += 3;
    }
}

void readBMP(const char *filename, int *width, int *height, unsigned char *pixels) {
    int fd_bmp = open(filename , O_RDONLY ); 
    if (-1 == fd_bmp)
    {
        // perror("图片文件打开失败");
        fprintf( stderr ,"图片文件:%s 打开失败\n" ,fd_bmp );
        return;
    }
    // 读取图片的信息
    BITMAPFILEHEADER file_head ; // 定义文件结构体
    int ret_val = read(fd_bmp , &file_head , sizeof(file_head));
    if (ret_val == -1 )
    {
        printf("读取文件头失败！！\n");
        return;
    }
    
    BITMAPINFOHEADER info_head ; // 定义信息头结构体
    ret_val = read(fd_bmp , &info_head , sizeof(info_head));
    if (ret_val == -1 )
    {
        printf("读取信息头失败！！\n");
    }
    char type[2];
    memcpy(type ,&file_head.bfType , 2 );
    printf("图像类型：%c%c\n" , type[0],type[1] );
    printf("图像宽度：%d\n" , info_head.biWidth );
    printf("图像高度：%d\n" , info_head.biHeight );
    printf("图像色深：%d\n" , info_head.biBitCount );
  	printf("函数%s\n" , __FUNCTION__ );

    *width = info_head.biWidth;
    *height = info_head.biHeight;

	ret_val = read(fd_bmp , pixels  , 640 * 640 * 3 );
    if (ret_val == -1 )
    {
        printf("读取颜色信息失败！！\n");
    }
	close(fd_bmp);
	printf("函数%d\n" , __LINE__ );
}

void image_put_pixel(uint8_t * image_data, int x, int y, unsigned int color)
{
	uint8_t *pen_24 = image_data+y*line_width+x*pixel_width;
	uint8_t red, green, blue;	
	red   = (color >> 16) & 0xff;
	green = (color >> 8) & 0xff;
	blue  = (color >> 0) & 0xff;
	*pen_24 = red;
	*(pen_24+1) = green;
	*(pen_24+2) = blue;
}

void image_put_ascii(uint8_t * image_data, int x, int y,  unsigned char c)
{
	unsigned char *dots = (unsigned char *)&fontdata_8x16[c*16];
	int i, b;
	unsigned char byte;

	for (i = 0; i < 16; i++)
	{
		byte = dots[i];
		for (b = 7; b >= 0; b--)
		{
			if (byte & (1<<b))
			{
				/* show */
				image_put_pixel(image_data, x+7-b, y+i, 0xffffff); /* 红*/
			}
			else
			{
				/* hide */
				image_put_pixel(image_data, x+7-b, y+i, 0x0000ff); /* 黑 */
			}
		}
	}
}

void image_set_str(uint8_t * image_data, int x, int y, char *data)
{
	
	int char_num = strlen(data);
	int i;
	for(i=0; i<char_num ;i++)
	{
		image_put_ascii(image_data,x+i*8,y,data[i]);
	}
}

void image_put_chinese(uint8_t * image_data, int x, int y, unsigned char *str)
{
	unsigned int area  = str[0] - 0xA1;
	unsigned int where = str[1] - 0xA1;
	unsigned char *dots = hzkmem + (area * 94 + where)*32;
	unsigned char byte;

	int i, j, b;
	for (i = 0; i < 16; i++)
		for (j = 0; j < 2; j++)
		{
			byte = dots[i*2 + j];
			for (b = 7; b >=0; b--)
			{
				if (byte & (1<<b))
				{
					/* show */
					image_put_pixel(image_data, x+j*8+7-b, y+i, 0xffff00); /* 白 */
				}
				else
				{
					/* hide */
					// image_put_pixel(image_data, x+j*8+7-b, y+i, 0); /* 黑 */
				}	
			}
		}
}

void image_put_chinese_str(uint8_t * image_data, int x, int y, unsigned char *str)
{
	int char_num = strlen(str);
	printf("size=%d\n",char_num);
	uint8_t signal_char[2];
	int i;
	for(i=0; i<char_num ;i+=2)
	{
		signal_char[0]=str[i];
		signal_char[1]=str[i+1];
		image_put_chinese(image_data, 200 + i*8,  200, signal_char);
	}
}

void init_font()
{
    fd_hzk16 = open("HZK16", O_RDONLY);
	if (fd_hzk16 < 0)
	{
		printf("can't open HZK16\n");
		return;
	}
	if(fstat(fd_hzk16, &hzk_stat))
	{
		printf("can't get fstat\n");
		return;
	}
	hzkmem = (unsigned char *)mmap(NULL , hzk_stat.st_size, PROT_READ, MAP_SHARED, fd_hzk16, 0);
	if (hzkmem == (unsigned char *)-1)
	{
		printf("can't mmap for hzk16\n");
		return;
	}
    close(fd_hzk16);
}

void init_network(char **argv)
{
    sess = create_graph(argv[1]);
    input_tensors[0] = csinn_alloc_tensor(NULL);
    input_tensors[0]->dim_count = 4;
    input_tensors[0]->dim[0] = 1;
    input_tensors[0]->dim[1] = 3;
    input_tensors[0]->dim[2] = 384;
    input_tensors[0]->dim[3] = 640;
    rgbData = shl_mem_alloc_aligned(1 * 3 * 480 * 640, 0);
    input_size = csinn_tensor_byte_size(((struct csinn_session *)sess)->input[i]);
    input_aligned = shl_mem_alloc_aligned(input_size, 0);
    scaledRGBData = shl_mem_alloc_aligned(input_size, 0);
}

void deinit_network()
{
    csinn_free_tensor(input_tensors[0]);
    shl_mem_free(input_aligned);
    shl_mem_free(rgbData);
    csinn_session_deinit(sess);
    csinn_free_session(sess);
}

void reinit_network(char **argv)
{
    csinn_session_deinit(sess);
    csinn_free_session(sess);
    usleep(50000);
    sess = create_graph(argv[1]);
}

int main(int argc, char **argv)
{
    if (argc < 2 )
    {
        printf("Please set valide args: ./model.elf model.params "
               "[tensor1/image1 ...] [tensor2/image2 ...]\n");
        return -1;
    }
    init_camera(argv[2]);  // 初始化摄像头
    // init_font();
    init_socket();
    init_network(argv);
    packet = (uint8_t *)malloc(33000);

     // 初始化线程属性
    pthread_attr_init(&attr);
    // 设置线程调度策略为SCHED_FIFO（先进先出）
    pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
    // 设置线程优先级
    param.sched_priority = sched_get_priority_max(SCHED_FIFO);
    pthread_attr_setschedparam(&attr, &param);
    pthread_t inference_t;
    pthread_t post_process_t;
    while(1)
    {
        // if(counter == 2000)
        // {
        //     printf("restart yolov5\n");
        //     reinit_network(argv);
        //     counter = 0;
        //     printf("restart yolov5 successfully\n");
        // }
        // counter++;
        /* poll */
        memset(fds, 0, sizeof(fds));
        fds[0].fd = fd;
        fds[0].events = POLLIN;
        if (1 == poll(fds, 1, -1))
        {
            start_time = shl_get_timespec();
            /* 把buffer取出队列 */
            struct v4l2_buffer buf;
            memset(&buf, 0, sizeof(struct v4l2_buffer));
            buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
            buf.memory = V4L2_MEMORY_MMAP;
            
            if (0 != ioctl(fd, VIDIOC_DQBUF, &buf))
            {
                perror("Unable to dequeue buffer");
                // break;
            }
    
            YUYVToRGB((const uint8_t*)(bufs[buf.index]), 640, 480, rgbData);

            // 缩放 RGB 数据并补零
            ScaleAndPadRGB(rgbData, 640,480, scaledRGBData, outputWidth, outputHeight);
            // image_preprocess(rgbData, 480, 640, 384, 640, scaledRGBData);

            transpose_image_int(scaledRGBData, input_aligned, 640, 384, 3);   //维度转换
           
            //对图像进行二次处理再次实现
            input_tensors[0]->data = input_aligned;
            // end_time = shl_get_timespec();
            // printf("Preprocess execution time: %.5fms, FPS=%.2f\n", ((float)(end_time - start_time)) / 1000000,
            //     1000000000.0 / ((float)(end_time - start_time)));
           
            // start_time = shl_get_timespec();
          
            // csinn_update_input_and_run(input_tensors, sess);
            // end_time = shl_get_timespec();
            // printf("inference execution time: %.5fms, FPS=%.2f\n", ((float)(end_time - start_time)) / 1000000,
            //     1000000000.0 / ((float)(end_time - start_time)));
            pthread_create(&inference_t, &attr, inferenceThread, NULL);
            pthread_create(&post_process_t, &attr, postProcessThread,NULL);
            // start_time = shl_get_timespec();
            // postprocess_opt(sess);
            int i; 
            for(i=0;i<result_num;i++)
            {
                
                if (result_w[i] != 0 && result_h[i] != 0 && result_x[i] > 0 && result_y[i] > 0)
                {
                    // 在 RGB 数据上绘制矩形框
                    DrawRectangleOutline(scaledRGBData, outputWidth, outputHeight, result_x[i], result_y[i], result_w[i], result_h[i]);
                    char result[32];
                    sprintf(result, "%s:0.%d", objects[result_label[i]], result_score[i]);
                    image_set_str(scaledRGBData, result_x[i], result_y[i], result);
                    result_x[i] = 0;
                    result_h[i] = 0;
                    result_w[i] = 0;
                    result_h[i] = 0;
                }
                end_time = shl_get_timespec();
                char result[32];
                sprintf(result, "FPS=%f",  1000000000.0 / ((float)(end_time - start_time)));
                image_set_str(scaledRGBData, 10, 10, result);
                // image_put_chinese_str(scaledRGBData, 0, 0, "帧率:");
                result_num = 0;
            }  
           
            printf("all execution time: %.5fms, FPS=%.2f\n", ((float)(end_time - start_time)) / 1000000,
                1000000000.0 / ((float)(end_time - start_time)));
            // SaveRGBToBMP(filename, scaledRGBData, outputWidth, outputHeight);
            // sendYolov5ImageToTargetHost(scaledRGBData, outputWidth*outputHeight*3);
            pthread_t send_thread;
            pthread_create(&send_thread, &attr, sendYolov5ImageToTargetHostThread, (void*)scaledRGBData);
           
            // if (start_flag1)
            // {
            //     start_flag1 = 0;
            //     // 写入PNG文件
            //     write_png_file("output.png", outputWidth, outputHeight, bit_depth, color_type, convert_to_png_bytep(scaledRGBData, outputWidth, outputHeight));
            // }
            // // 将RGB数组转换为PNG格式并返回PNG数据
            // png_data = convert_rgb_to_png(outputWidth, outputHeight, bit_depth, color_type, scaledRGBData);
            // printf("png size=%d",png_data.size);

           
           
     
            /* 把buffer放入队列 */
            if (0 != ioctl(fd, VIDIOC_QBUF, &buf))
            {
                perror("Unable to queue buffer");
            }
        }
    }
    // csinn_free_tensor(input_tensors[0]);

    // shl_mem_free(input_aligned);
    // shl_mem_free(rgbData);
    deinit_network();

    // csinn_session_deinit(sess);
    // csinn_free_session(sess);

    if (0 != ioctl(fd, VIDIOC_STREAMOFF, &type))
    {
        perror("Unable to stop capture");
        // return -1;
    }
    close(fd);
    // free(png_data.data);
    free(packet);
    munmap(hzkmem , hzk_stat.st_size);
    return 0;
}
